#include "foc_pid.h"


CURRENT_PID_DEF current_q_pid;
CURRENT_PID_DEF current_d_pid;
SPEED_PID_DEF speed_pid;  

static float Limit_Sat( float fdata,float fmax, float fmin) 
{
    fdata = (fdata > fmax) ? fmax : fdata;
    fdata = (fdata < fmin) ? fmin : fdata;

    return fdata;
}

void current_q_pid_Init(CURRENT_PID_DEF *current_temp, float motor_Ls, float motor_Rs, float Vbus)
{ 

	//Normal PI Controller
		current_temp->kp = motor_Ls * 200.0f * 6.18f * 0.4f;  //bandwidth ≈ 200HZ
    current_temp->ki = motor_Rs * 200.0f * 6.18f * 0.0002f * 0.012f;
	
    current_temp->sum = 0.0f;
  	current_temp->ui = 0.0f;
    current_temp->maxout = Vbus * ONE_BY_SQRT3;
    current_temp->minout = -Vbus * ONE_BY_SQRT3;
}

void current_d_pid_Init(CURRENT_PID_DEF *current_temp, float motor_Ls, float motor_Rs, float Vbus)
{ 

	//Normal PI Controller
		current_temp->kp = motor_Ls * 200.0f * 6.18f * 0.3f;  //bandwidth ≈ 200HZ
    current_temp->ki = motor_Rs * 200.0f * 6.18f * 0.0002f * 0.012f;
	
    current_temp->sum = 0.0f;
  	current_temp->ui = 0.0f;
    current_temp->maxout = 2.0f;
    current_temp->minout = -2.0f;
}


void current_pid_Control(CURRENT_PID_DEF *current_temp)
{
	  //Normal PI Controller
    current_temp->err = current_temp->ref - current_temp->fbk;
    current_temp->up = current_temp->kp * current_temp->err;
    current_temp->ui = Limit_Sat(
                              current_temp->ui + current_temp->ki *current_temp->err, 
                              current_temp->maxout, 
                              current_temp->minout);
    current_temp->output = Limit_Sat(
                              current_temp->up + current_temp->ui, 
                              current_temp->maxout, 
                              current_temp->minout);
}


void  Speed_pid_c(SPEED_PID_DEF* speed_pid_temp)
{
		//Normal PI Controller
    speed_pid_temp->error = speed_pid_temp->ref - speed_pid_temp->fbk;
   speed_pid_temp->up  = speed_pid_temp->Kp * speed_pid_temp->error;
    speed_pid_temp->I_Sum= Limit_Sat(
															speed_pid_temp->I_Sum  +  speed_pid_temp->Ki  *speed_pid_temp->error, 
                              speed_pid_temp->Max_Output, 
                              speed_pid_temp->Min_Output);
   speed_pid_temp ->output = Limit_Sat(
                              speed_pid_temp->up + speed_pid_temp->I_Sum, 
                              speed_pid_temp->Max_Output, 
                              speed_pid_temp->Min_Output);
}

void speed_pid_init(SPEED_PID_DEF* speed_pid_temp, float motor_J, float motor_flux, unsigned int motor_Pole, unsigned int n)
{
	
	//Normal PI Controller
	speed_pid_temp -> Kp = 100.0f * 6.28f * motor_J / (1.5f * motor_Pole * motor_flux) * 12.0f;  //2; //* 4.0f;
	speed_pid_temp -> Ki = 100.0f * 6.28f * speed_pid.Kp * 0.00005f * n *0.1f;  //2;    // *0.1f;
	 
	speed_pid_temp -> Max_Output =  10.0f;   
	speed_pid_temp -> Min_Output = -5.0f;
	speed_pid_temp -> up = 0.0f;
	speed_pid_temp -> I_Sum = 0.0f;
}
